/*
 * main.cpp
 */

#include <iostream>
#include <time.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include <pthread.h>
#include <stdio.h>
#include <sys/time.h>
#include "mavlink.h"
#include "timer.h"
#include "Communication.h"
#include "EKF.h"
#include "Controller.h"
#include "execStruct.h"
#include "FileWriter.h"
#include "Guidance.h"

using namespace std;

// Declare global structures
aupData apdat;
controlVector cv;
stateVector sv;
paramVector pv;
controlGains ctrlGns;

int main() {

	// Flag to stop code running
	pv.killMariner = false;

	// Initialize classes
	FileWriter *fw     = new FileWriter();
	Timer *timer       = new Timer();
	Communication *com = new Communication(*timer, fw);
	EKF *ekf           = new EKF(fw);
	Guidance *gnc      = new Guidance(fw,com->fgnd);
	Controller *cntrl  = new Controller(com->faup,com->fgnd, fw, gnc);

	timer->addListenerOneHz(com);
	timer->addListenerFiveHz(com);
	timer->addListenerFiftyHz(cntrl);

	com->addListenerAUP(ekf);
	com->addListenerGND(cntrl);
	com->addListenerGND(gnc);

	// Start timer
	timer->startTiming();

	sleep(100000000);

	timer->stopTiming();
	com->closeSerialPorts();
	fw->closeFile();

	cout << "Done" << endl;
	return 0;
};
